// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceBoxGrip.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.PicknPlace
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.PhysicalObjects;

    /// <summary>
    /// Implements IObjectGrip for box objects
    /// </summary>
    public class PicknPlaceBoxGrip : PicknPlaceObjectGrip
    {
        /// <summary>
        /// Set of equivalent grip styles
        /// </summary>
        private HashSet<GripStyle> equivalentGripStyleHash = new HashSet<GripStyle>() { GripStyle.WrapAroundPrincipalAxisFace1, GripStyle.WrapAroundPrincipalAxisFace2 };

        /// <summary>
        ///  Initializes a new instance of the <see cref="PicknPlaceBoxGrip" /> class
        /// </summary>
        /// <param name="physicalObject">The object description</param>
        public PicknPlaceBoxGrip(PhysicalObjectDescription physicalObject)
            : base(physicalObject)
        {
            this.SupportedGripStyles = new List<GripStyle>() 
                        {
                            GripStyle.WrapAroundPrincipalAxisFace1, 
                            GripStyle.WrapAroundPrincipalAxisFace2,
                            GripStyle.TopDownAroundPrincipalAxis
                        };
        }

        /// <summary>
        /// Returns an enumerator for the collection
        /// </summary>
        /// <returns>Enumerator for the collection</returns>
        public override IEnumerator<GripStyle> GetEnumerator()
        {
            this.Reset();
            return this;
        }

        /// <summary>
        /// Returns the palm plane 
        /// </summary>
        /// <param name="gripStyle">The grip style</param>
        /// <param name="objectPose">Object's pose</param>
        /// <returns>plane normal direction and distance from origin</returns>
        public override Vector4 GetPalmPlane(GripStyle gripStyle, Pose objectPose)
        {
            Vector3 n = this.GetPalmPlaneNormal(objectPose, gripStyle);
            Vector4 plane = this.PlaneDistancefromOrigin(n, this.GetGripCenter(gripStyle, objectPose));
            return plane;
        }

        /// <summary>
        /// Get the grip axis - the axis parallel to the palm plane running between thumb and fingers
        /// around which grip closes. For example with box wraparound grip it is parallel to the 
        /// box face that wrap around grips (that is 
        /// </summary>
        /// <param name="gripStyle">The grip type</param>
        /// <param name="objectPose">Object's pose in global coordinates</param>
        /// <returns>The grip axis</returns>
        public override Vector3 GetGripAxis(GripStyle gripStyle, Pose objectPose)
        {
            Vector3 gripAxis;

            switch (gripStyle)
            {
                case GripStyle.WrapAroundPrincipalAxisFace1:
                case GripStyle.WrapAroundPrincipalAxisFace2:
                    gripAxis = Quaternion.Rotate(objectPose.Orientation, new Vector3(0, 0, 1));
                    break;

                case GripStyle.TopDownAroundPrincipalAxis:
                    gripAxis = this.GetPalmPlaneNormal(objectPose, GripStyle.WrapAroundPrincipalAxisFace1);
                    break;

                default:
                    throw new ArgumentException(string.Format("Grip style {0} is not supported by box grips", gripStyle.ToString()));
            }

            return gripAxis;
        }

        /// <summary>
        /// Gets the centre point of the grip along the axis
        /// </summary>
        /// <param name="gripStyle">The grip type</param>
        /// <param name="objectPose">Object's pose</param>
        /// <returns>Center grip point</returns>
        public override Vector3 GetGripCenter(GripStyle gripStyle, Pose objectPose)
        {
            double offsetAlongNormal = 0;

            switch (gripStyle)
            {
                case GripStyle.WrapAroundPrincipalAxisFace1:
                case GripStyle.WrapAroundPrincipalAxisFace2:
                    offsetAlongNormal = this.ManipulatedObjectDescription.WidthInM * 5 / 8;
                    break;

                case GripStyle.TopDownAroundPrincipalAxis:
                    offsetAlongNormal = this.ManipulatedObjectDescription.HeightInM * 2 / 3;
                    break;

                default:
                    throw new ArgumentException(string.Format("Grip style {0} is not supported by box grips", gripStyle.ToString()));
            }

            Vector3 gripOffset = this.GetPalmPlaneNormal(objectPose, gripStyle) * offsetAlongNormal;
            Vector3 gripCenter = objectPose.Position - gripOffset;
            return gripCenter;
        }

        /// <summary>
        /// Gets an estimate of the position of the horizontal "table" surface from the object pose
        /// This is a temporary approach to obstacle (surface) avoidance. It will be replaced by 
        /// a more comprehensive obstacle avoidance
        /// </summary>
        /// <param name="objectPose">Object pose</param>
        /// <returns>Distance to the horizontal surface</returns>
        public override double GetHorizontalPlaneOffset(Pose objectPose)
        {
            return objectPose.Position.Z - (this.ManipulatedObjectDescription.HeightInM / 2.0);
        }

        /// <summary>
        /// Gets a suggested gripper opening in M for the grip style
        /// </summary>
        /// <param name="gripStyle">Grip style</param>
        /// <returns>Gripper opening in M</returns>
        public override double GetGripperOpeningInM(GripStyle gripStyle)
        {
            return this.ManipulatedObjectDescription.DepthInM;
        }

        /// <summary>
        /// Obtain a set of equivalent grip styles. Grip styles in an
        /// equivalent set can be pick or placed with out having to make
        /// grip change. For example Grip from top or grip from bottom can be equivalent
        /// for a symmetric object
        /// </summary>
        /// <param name="gripStyle">Sample element for equivalent set</param>
        /// <returns>Set of equivalent grip styles</returns>
        public override IEnumerable<GripStyle> GetEquivalentGripStyleSet(GripStyle gripStyle)
        {
            GripStyle[] set = null;

            if (true == this.equivalentGripStyleHash.Contains(gripStyle))
            {
                set = new GripStyle[this.equivalentGripStyleHash.Count];
                this.equivalentGripStyleHash.CopyTo(set);
            }
            else
            {
                set = new GripStyle[] { gripStyle };
            }

            return set;
        }

        /// <summary>
        /// Description of the object
        /// </summary>
        /// <returns>String description</returns>
        public override string ToString()
        {
            return "Box";
        }

        /// <summary>
        /// Gets the palm plan normal from object pose for the given grip style
        /// </summary>
        /// <param name="objectPose">Object pose</param>
        /// <param name="gripStyle">Grip style</param>
        /// <returns>Palm plane normal</returns>
        private Vector3 GetPalmPlaneNormal(Pose objectPose, GripStyle gripStyle)
        {
            Vector3 baseDirection;
            switch (gripStyle)
            {
                case GripStyle.TopDownAroundPrincipalAxis:
                    baseDirection = new Vector3(0, 0, -1);
                    break;

                case GripStyle.WrapAroundPrincipalAxisFace1:
                    baseDirection = new Vector3(1, 0, 0);
                    break;

                case GripStyle.WrapAroundPrincipalAxisFace2:
                    baseDirection = new Vector3(-1, 0, 0);
                    break;

                default:
                    throw new ArgumentException(string.Format("Grip style {0} is not supported by box grips", gripStyle.ToString()));
            }

            Vector3 n = Vector3.Normalize(Quaternion.Rotate(objectPose.Orientation, baseDirection));
            return n;
        }
    }
}
